#include "module_config.h"
#include "stm32g4xx.h"
#include "wl_version.h"

#if 0
static void powerOnPush_delayms(uint16_t ms)
{
    uint64_t curTimestamp = 0;

    curTimestamp = getSystemTimestamp();
    while (getSystemTimestamp() < curTimestamp+ms);
}

/* push data to tx ring */
static void __packData(uint8_t* ps8DstBuf, uint8_t *pu8SrcData, uint8_t u8SrcDataSize, uint8_t u8Index, uint8_t u8Cmd, uint8_t u8SrcId, uint8_t u8DstId)
{
    uint16_t  u16Crc;

    ps8DstBuf[0] = 0x5A;
    ps8DstBuf[1] = 0xA5;
    ps8DstBuf[2] = u8SrcDataSize;
    ps8DstBuf[3] = u8SrcId;
    ps8DstBuf[4] = u8DstId;
    ps8DstBuf[5] = u8Cmd;
    ps8DstBuf[6] = u8Index;

    if ((NULL != pu8SrcData) && (0 != u8SrcDataSize))
    {
        memcpy(&ps8DstBuf[7], pu8SrcData, u8SrcDataSize);
    }

    u16Crc = GetCrc16((uint8_t*)&ps8DstBuf[2], u8SrcDataSize + 5);

    ps8DstBuf[u8SrcDataSize + 7] = u16Crc & 0xff;
    ps8DstBuf[u8SrcDataSize + 8] = (u16Crc >> 8) & 0xff;
}

static void powerOnByPush_sendDataToMotor(COMM_TYPE_ENUM type, uint8_t *data, uint8_t u8DataSize, uint8_t u8Index, uint8_t u8Cmd, uint8_t u8SrcId, uint8_t u8DstId)
{
    if (type >= COMM_MAX)
    {
        return;
    }

    memset(&commEntity[type].tx.buf[0], 0, commEntity[type].tx.dmaBufSize);
    __packData(&commEntity[type].tx.buf[0], data, u8DataSize, u8Index, u8Cmd, u8SrcId, u8DstId);
    HAL_UART_Transmit(commEntity[type].comm, commEntity[type].tx.buf, u8DataSize+9, 10);
}

static void powerOnByPush_setMotorDevId(COMM_TYPE_ENUM type)
{
    uint8_t idTable[2] = {0};
    uint8_t size = 0;

    switch (type)
    {
        case VCU1_COMM_TYPE:
            idTable[0] = ID_VCU1;
            size = 1;
            break;

        case VCU2_COMM_TYPE:
            idTable[0] = ID_VCU2;
            size = 1;
            break;

        default:
            return;
    }

    powerOnByPush_sendDataToMotor(type, idTable, size, COMM_MOTOR_ID_INDEX, COMM_CMD_CAMP_WR_NR, ID_SELF, 0x00);
}

static void powerOnByPush_setMotorEmergency(COMM_TYPE_ENUM type)
{
    uint8_t data[18] = {0};
    int16_t iq = 1000;

    switch (type)
    {
        case VCU1_COMM_TYPE:
            data[0] = (SPEED_LOOP_MODE|0x01<<3) | ((SPEED_LOOP_MODE|0x01<<3)<<4); /* rear driver  enable/speedLoop */
            memcpy(&data[1], (uint8_t*)&iq, sizeof(int16_t));
            memcpy(&data[3], (uint8_t*)&iq, sizeof(int16_t));
            powerOnByPush_sendDataToMotor(type, data, 17, COMM_MOTOR_CMD_INDEX, COMM_CMD_CAMP_WR_NR, ID_SELF, ID_VCU1);
            break;

        case VCU2_COMM_TYPE:
            data[0] = (SPEED_LOOP_MODE|0x01<<3) | ((POS_LOOP_MODE|0x00<<3)<<4); /* front driver  enable/speedLoop  turn motor must disable*/
            memcpy(&data[1], (uint8_t*)&iq, sizeof(int16_t));
            memcpy(&data[3], (uint8_t*)&iq, sizeof(int16_t));
            powerOnByPush_sendDataToMotor(type, data, 17, COMM_MOTOR_CMD_INDEX, COMM_CMD_CAMP_WR_NR, ID_SELF, ID_VCU2);
            break;

        default:
            break;
    }
}

static void powerOnByPush_motorProc(void)
{
    powerOnByPush_setMotorDevId(VCU1_COMM_TYPE);
    powerOnByPush_setMotorDevId(VCU2_COMM_TYPE);

    powerOnByPush_setMotorEmergency(VCU1_COMM_TYPE);
    // powerOnByPush_setMotorEmergency(VCU2_COMM_TYPE);
}

void powerOnByPushLoop(void)
{
    uint8_t pushPowerOnFlag = 0;                /* 0: normal  1:push with battery   2:push without battery */
    uint8_t isPush = 0xFF;
    
    printf("\n");
    while(1)
    {
        if (isPush != POWER_SOURCE_BY_PUSH())
        {
            isPush = POWER_SOURCE_BY_PUSH();
            powerOnPush_delayms(10);
            continue;
        }

        /* not push */
        if (isPush == 0)
        {
            printf("powerOnByPushLoop not push\n");
            switch (pushPowerOnFlag)
            {
                case 0:
                default:
                    break;

                case 1:
                    power_ctrl(ECU_5V0_PERIPHERAL_EN, 0);
                    power_ctrl(ECU_MOTOR_DRIVER_PWR_EN, 0);
                    power_ctrl(ECU_VCU_PWR_EN,0);
                    power_ctrl(ECU_VCU_5V0_EN, 0);
                    powerOnPush_delayms(500);

                    /* not do this */
                    powerOnPush_delayms(1500);
                    HAL_NVIC_SystemReset();
                    break;

                case 2:
                    /* notify vcu */
                    powerOnPush_delayms(100);
                    power_ctrl(ECU_5V0_PERIPHERAL_EN, 0);
                    power_ctrl(ECU_MOTOR_DRIVER_PWR_EN, 0);
                    power_ctrl(ECU_VCU_5V0_EN, 0);
                    power_ctrl(ECU_VCU_PWR_EN,0);
                    powerOnPush_delayms(500);

                    /* not do this */
                    powerOnPush_delayms(1500);
                    HAL_NVIC_SystemReset();
                    break;
            }

            pushPowerOnFlag = 0;
            return;
        }

        /* check bms first while push start */
        if (pushPowerOnFlag == 0)
        {
            pushPowerOnFlag = 2;
            MX_I2C1_Init();
            for(uint8_t i= 0; i<2; i++)
            {
                powerOnPush_delayms(50);
                if (BQ76942_CheckHardware() == 0)
                {
                    pushPowerOnFlag = 1;
                    break;
                }
            }

            if (pushPowerOnFlag == 1)
            {
                BAT_SW_ENABLE();
                printf("powerOnByPushLoop with battery\n");
            }

            power_ctrl(ECU_MOTOR_DRIVER_PWR_EN, 1);
            power_ctrl(ECU_VCU_PWR_EN,1);
            MX_USART3_UART_Init();
            MX_USART6_UART_Init();
        }

        if (pushPowerOnFlag != 0)
        {
            powerOnByPush_motorProc();
            powerOnPush_delayms(100);
        }

        if (POWER_OCD_READ() == 1)
        {
            printf("over current release\n");
            POWER_OCD_RELEASE_WRITE(1);
            powerOnPush_delayms(1);
            POWER_OCD_RELEASE_WRITE(0);
        }
    }
}
#endif
